﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace BoardController.Helper
{
    public class PID
    {
        private VitualDevice vitual;

        private IMU leftimu;
        private IMU rightimu;

        private const int size = 10;
        public List<int> FanData = new List<int>();

        public DataFilter LeftArray = new DataFilter(size);
        public DataFilter RightArray = new DataFilter(size);

        public DataFilter FrontLeftArray = new DataFilter(size);
        public DataFilter FrontRightArray = new DataFilter(size);
        public DataFilter BackLeftArray = new DataFilter(size);
        public DataFilter BackRightArray = new DataFilter(size);


        private AsyncServer netserver;
        public PID(AsyncServer _netserver, List<Device> _devices)
        {
            vitual = (VitualDevice)_devices[0];
            leftimu = (IMU)_devices[1];
            rightimu = (IMU)_devices[2];

            for (int i = 0; i < 6; i++)
            {
                FanData.Add(0);
            }
            netserver = _netserver;
        }

        public void Run(bool flag, int label)
        {
            /////////////////////////////////////////////////
            //Debug.Print(leftimu.FilteredXLight.GetCurrentValue().ToString() + "\t" + rightimu.FilteredXLight.GetCurrentValue().ToString());
            /////////////////////////////////////////////////

            // IMU初始数据经过坐标变换后，向墙是y轴正方向，z是向上，右手坐标系
            // 光流数据x轴正方向为面向墙面右手方向

            double leftspeed = leftimu.FilteredXLight.Average();
            double rightspeed = rightimu.FilteredXLight.Average();
            double horizontal_speed = leftspeed;
            if (Math.Abs(rightspeed) < Math.Abs(leftspeed))
            {
                horizontal_speed = rightspeed;
            }
            double horizontal_acc = (leftimu.FilteredXACC.Average() + rightimu.FilteredXACC.Average()) / 2;

            var fans2 = LeftRightPID(horizontal_speed, horizontal_acc);

            double yaw_speed = (leftimu.FilteredZRotSpeed.Average() + leftimu.FilteredZRotSpeed2.Average()
                            + leftimu.FilteredZRotSpeed.Average() + leftimu.FilteredZRotSpeed2.Average()) / 4;

            // Console.WriteLine(string.Format("yaw_speed: {0}", yaw_speed));

            double yawSpeedByDepth = Manifest.YawSpeedByDepth.Average();

            // 调节扭动
            var yaw_fans4 = YawPIDDepth(yawSpeedByDepth, yaw_speed);

            //var yaw_fans4 = YawPIDIMU(yaw_angel, yaw_speed);

            double yaw = (leftimu.Yaw + rightimu.Yaw) / 2;
            double pitch = (leftimu.Pitch + rightimu.Pitch) / 2;
            double roll = (leftimu.Roll + rightimu.Roll) / 2;

            vitual.YawCurrent = yaw;
            vitual.PitchCurrent = pitch;
            vitual.RollCurrent = roll;

            // 调节远近
            double nearfar_acc = (leftimu.FilteredYACC.Average() + 0.00693 + rightimu.FilteredYACC.Average() + 0.0125) / 2;

            // 输入速度单位必须为 m/s
            double speedLeaveWall = Manifest.SpeedLeaveWall.Average();
            var farnear_fans4 = FarNearPID(speedLeaveWall, nearfar_acc);

            int addrDev = 0x03;
            int function = 0x10;
            int addrReg = 0x10;
            int RegsCount = 6;

            int leftright_offset = 20;
            // int leftright_offset = 0;
            int front_offset = 20;
            int back_offset = 15;
            //int front_offset = 0;
            //int back_offset = 0;

            int left = fans2[0];
            int right = fans2[1];

            int fl = yaw_fans4[0] > farnear_fans4[0] ? yaw_fans4[0] : farnear_fans4[0];
            int fr = yaw_fans4[1] > farnear_fans4[1] ? yaw_fans4[1] : farnear_fans4[1];
            int bl = yaw_fans4[2] > farnear_fans4[2] ? yaw_fans4[2] : farnear_fans4[2];
            int br = yaw_fans4[3] > farnear_fans4[3] ? yaw_fans4[3] : farnear_fans4[3];

            left = left + leftright_offset;
            right = right + leftright_offset;
            int left_base = fl > bl ? bl : fl;
            int right_base = fr > br ? br : fr;
            fl = fl - left_base + front_offset;
            fr = fr - right_base + front_offset;
            bl = bl - left_base + back_offset;
            br = br - right_base + back_offset;

            left = Math.Min(left, 80);
            right = Math.Min(right, 80);
            fl = Math.Min(fl, 50);
            fr = Math.Min(fr, 50);
            bl = Math.Min(bl, 50);
            br = Math.Min(br, 50);

            LeftArray.Insert(left);
            RightArray.Insert(right);
            FrontLeftArray.Insert(fl);
            FrontRightArray.Insert(fr);
            BackLeftArray.Insert(bl);
            BackRightArray.Insert(br);

            List<int> data = new List<int>();
            double leftFanV = LeftArray.Average();
            double rightFanV = RightArray.Average();
            double frontLeftV = FrontLeftArray.Average();
            double frontRightV = FrontRightArray.Average();
            double backLeftV = BackLeftArray.Average();
            double backRightV = BackRightArray.Average();

            data.Add(Convert.ToInt32(LeftArray.Average()));
            data.Add(Convert.ToInt32(RightArray.Average()));

            //data.Add(Convert.ToInt32(20));
            //data.Add(Convert.ToInt32(20));
            data.Add(Convert.ToInt32(FrontLeftArray.Average()));
            data.Add(Convert.ToInt32(FrontRightArray.Average()));
            data.Add(Convert.ToInt32(BackLeftArray.Average()));
            data.Add(Convert.ToInt32(BackRightArray.Average()));

            FanData = data;
            if (flag)
            {
                if (!Manifest.Emergency)
                {
                    netserver.Send(addrDev, function, addrReg, RegsCount, data, label);
                }
                else
                {
                    List<int> empty = new List<int>();
                    empty.Add(0);
                    empty.Add(0);
                    empty.Add(0);
                    empty.Add(0);
                    empty.Add(0);
                    empty.Add(0);
                    netserver.Send(addrDev, function, addrReg, RegsCount, empty, label);
                }
            }
            else
            {

            }
        }

        public List<int> LeftRightPID(double speed, double acc)
        {
            // Debug.Print(string.Format("speed: {0}---acc: {1}", speed, acc));
            double ratio = Math.Abs(speed) / (Math.Abs(acc) + 0.01);
            if (ratio > 3000)
            {
                List<int> temp = new List<int>();
                temp.Add(0);
                temp.Add(0);
                return temp;
            }

            double d2pratio = 1;
            // double wp = 1; // 1 / (1 + d2pratio);
            double wp = 0.6;
            // double wd = 0;
            double wd = 0.5; // d2pratio / (1 + d2pratio);

            double speed_abs = Math.Abs(speed);
            double acc_abs = Math.Abs(acc);

            double speed_max = 50;
            double acc_max = 1.2;

            double speed_scale = 3;
            double acc_scale = 60;  //80

            if (speed_abs > speed_max)
            {
                speed_abs = speed_max;
            }
            if (acc_abs > acc_max)
            {
                acc_abs = acc_max;
            }

            double pleft = 0;
            double pright = 0;

            double dleft = 0;
            double dright = 0;

            if (speed < 0)
            {
                pleft = speed_abs * speed_scale;
                pright = 0;
            }
            else
            {
                pleft = 0;
                pright = speed_abs * speed_scale;
            }

            if (acc < 0)
            {
                dleft = acc_abs * acc_scale;
                //dleft = 10;
                dright = 0;
            }
            else
            {
                dleft = 0;
                dright = acc_abs * acc_scale;
                //dright = 10;
            }
            int left = (int)(pleft * wp + dleft * wd);
            int right = (int)(pright * wp + dright * wd);
            List<int> data = new List<int>();
            data.Add(left);
            data.Add(right);
            //data.Add(0);
            //data.Add(0);
            return data;
        }

        /// <summary>
        /// 调节扭动
        /// </summary>
        /// <param name="depth_yawspeed"></param>
        /// <param name="imu_yawspeed"></param>
        /// <returns></returns>
        public List<int> YawPIDDepth(double depth_yawspeed, double imu_yawspeed)
        {
            // Debug.Print(string.Format("depth_yawspeed: {0}---imu_yawspeed: {1}", depth_yawspeed, imu_yawspeed));
            double d2pratio = 4;

            // 目前只用到imu数据
            // double wp = 0; // / (1 + d2pratio);
            // double wd = 1; // d2pratio / (1 + d2pratio);

            double wp = 0.3; // 0.6
            double wd = 0.1;  //

            double depth_base = 0;
            double imu_base = 0;

            double depth_delta = depth_yawspeed - depth_base;
            double imu_delta = imu_yawspeed - imu_base;
            double depth_abs = Math.Abs(depth_delta);
            double imu_abs = Math.Abs(imu_delta);

            double depth_max = 30;
            double imu_max = 3;

            double depth_scale = 1;
            double imu_scale = 20;

            if (depth_abs > depth_max)
            {
                depth_abs = depth_max;
            }
            if (imu_abs > imu_max)
            {
                imu_abs = imu_max;
            }

            double pfrontleft = 0;
            double pfrontright = 0;
            double pbackleft = 0;
            double pbackright = 0;

            double dfrontleft = 0;
            double dfrontright = 0;
            double dbackleft = 0;
            double dbackright = 0;

            if (depth_delta < 0)
            {
                pfrontleft = depth_abs * depth_scale;
                pfrontright = 0;
                pbackleft = 0;
                pbackright = depth_abs * depth_scale;
            }
            else
            {
                pfrontleft = 0;
                pfrontright = depth_abs * depth_scale;
                pbackleft = depth_abs * depth_scale;
                pbackright = 0;
            }

            // 提供抵消当前重力加速度分量相反方向的加速度
            if (imu_delta < 0)
            {
                dfrontleft = imu_abs * imu_scale;
                dfrontright = 0;
                dbackleft = 0;
                dbackright = imu_abs * imu_scale;
            }
            else
            {
                dfrontleft = 0;
                dfrontright = imu_abs * imu_scale;
                dbackleft = imu_abs * imu_scale;
                dbackright = 0;
            }

            List<int> data = new List<int>();

            int fl = (int)(pfrontleft * wp + dfrontleft * wd);
            int fr = (int)(pfrontright * wp + dfrontright * wd);
            int bl = (int)(pbackleft * wp + dbackleft * wd);
            int br = (int)(pbackright * wp + dbackright * wd);

            data.Add(fl);
            data.Add(fr);
            data.Add(bl);
            data.Add(br);

            //data.Add(0);
            //data.Add(0);
            //data.Add(0);
            //data.Add(0);

            return data;
        }

        public List<int> YawPIDIMU(double yawangle, double yawspeed)
        {
            double d2pratio = 4;
            double wp = 1 / (1 + d2pratio);
            double wd = d2pratio / (1 + d2pratio);

            double angle_base = vitual.YawBase;
            double speed_base = 0;

            double angle_delta = yawangle - angle_base;
            double speed_delta = yawspeed - speed_base;
            double angle_abs = Math.Abs(angle_delta);
            double speed_abs = Math.Abs(speed_delta);

            double angle_max = 3;
            double speed_max = 8;

            double angle_scale = 30;
            double speed_scale = 11;

            if (angle_abs > angle_max)
            {
                angle_abs = angle_max;
            }
            if (speed_abs > speed_max)
            {
                speed_abs = speed_max;
            }

            double pfrontleft = 0;
            double pfrontright = 0;
            double pbackleft = 0;
            double pbackright = 0;

            double dfrontleft = 0;
            double dfrontright = 0;
            double dbackleft = 0;
            double dbackright = 0;

            if (angle_delta < 0)
            {
                pfrontleft = angle_abs * angle_scale;
                pfrontright = 0;
                pbackleft = 0;
                pbackright = angle_abs * angle_scale;
            }
            else
            {
                pfrontleft = 0;
                pfrontright = angle_abs * angle_scale;
                pbackleft = angle_abs * angle_scale;
                pbackright = 0;
            }

            if (speed_delta < 0)
            {
                dfrontleft = speed_abs * speed_scale;
                dfrontright = 0;
                dbackleft = 0;
                dbackright = speed_abs * speed_scale;
            }
            else
            {
                dfrontleft = 0;
                dfrontright = speed_abs * speed_scale;
                dbackleft = speed_abs * speed_scale;
                dbackright = 0;
            }

            List<int> data = new List<int>();

            int fl = (int)(pfrontleft * wp + dfrontleft * wd);
            int fr = (int)(pfrontright * wp + dfrontright * wd);
            int bl = (int)(pbackleft * wp + dbackleft * wd);
            int br = (int)(pbackright * wp + dbackright * wd);

            data.Add(fl);
            data.Add(fr);
            data.Add(bl);
            data.Add(br);

            //data.Add(0);
            //data.Add(0);
            //data.Add(0);
            //data.Add(0);

            return data;
        }

        /// <summary>
        /// 经坐标变换后，IMU y轴正向垂直墙面，x轴正向向右，右手坐标系
        /// 目标: PID调节速度项为0，加速度为微分项
        /// </summary>
        /// <param name="speed"></param>
        /// <param name="acc"></param>
        /// <returns></returns>
        public List<int> FarNearPID(double speed, double acc)
        {
            // Debug.Print("FarNearPID, speed: {0}, acc: {1}", speed, acc);
            double d2pratio = 4;
            //double wp = 0.9; // 1 / (1 + d2pratio);
            //double wd = 0.1; // d2pratio / (1 + d2pratio);

            double wp = 0.4;
            double wd = 0; // 0.2
            double accAbs = Math.Abs(acc);
            if (accAbs < 0.01)
            {
                wd = 0;
            }

            double speed_max = 50;
            double acc_max = 1.2;

            //double speed_scale = 5;
            double speed_scale = 2.5;
            double acc_scale = 120;

            double speed_base = 0;
            double acc_base = 0;

            speed = speed - speed_base;
            double speed_abs = Math.Abs(speed);

            acc = acc - acc_base;
            double acc_abs = Math.Abs(acc);

            if (speed_abs > speed_max)
            {
                speed_abs = speed_max;
            }
            if (acc_abs > acc_max)
            {
                acc_abs = acc_max;
            }

            double pfrontleft = 0;
            double pfrontright = 0;
            double pbackleft = 0;
            double pbackright = 0;

            double dfrontleft = 0;
            double dfrontright = 0;
            double dbackleft = 0;
            double dbackright = 0;

            // 靠近墙面
            if (speed < 0)
            {
                pfrontleft = speed_abs * speed_scale;
                pfrontright = speed_abs * speed_scale;
                pbackleft = 0;
                pbackright = 0;
            }
            else
            {
                pfrontleft = 0;
                pfrontright = 0;
                pbackleft = speed_abs * speed_scale;
                pbackright = speed_abs * speed_scale;
            }
            if (acc > 0)
            {
                dfrontleft = acc_abs * acc_scale;
                dfrontright = acc_abs * acc_scale;
                //dfrontleft = 10;
                //dfrontright = 10;
                dbackleft = 0;
                dbackright = 0;
            }
            else
            {
                dfrontleft = 0;
                dfrontright = 0;
                dbackleft = acc_abs * acc_scale;
                dbackright = acc_abs * acc_scale;
                //dbackleft = 10;
                //dbackright = 10;
            }

            List<int> data = new List<int>();

            int fl = (int)(pfrontleft * wp + dfrontleft * wd);
            int fr = (int)(pfrontright * wp + dfrontright * wd);
            int bl = (int)(pbackleft * wp + dbackleft * wd);
            int br = (int)(pbackright * wp + dbackright * wd);

            data.Add(fl);
            data.Add(fr);
            data.Add(bl);
            data.Add(br);

            //data.Add(0);
            //data.Add(0);
            //data.Add(0);
            //data.Add(0);
            return data;
        }
    }
}
